#include <stdlib.h>
#include <math.h>

//#include "tarm_timer.h"


#include "TARM_Hardware/tarm_io.h"

#include "TARM_Application/tarm_init.h"
#include "TARM_Application/tarm_run.h"
//#include "tarm_uart.h"


int InitFlag=0;

void set_current_start()
{
	g_TARM_RunRecord.pv_current.p = g_TARM_Factor.limit_p;
	g_TARM_RunRecord.pv_current.v = g_TARM_Factor.limit_v;

	angle_to_xy(g_TARM_RunRecord.pv_current.p, g_TARM_RunRecord.pv_current.v,
			&g_TARM_RunRecord.pv_current.x, &g_TARM_RunRecord.pv_current.y);
}

void io_init()
{
	X_DIR_SET;
	Y_DIR_RESET;
	Z_DIR_SET;

	X_STP_RESET;
	Y_STP_RESET;
	Z_STP_RESET;

	EN_STEER_RESET;
}

void locate_init()
{
	InitFlag = 0;

//	while(X_LIM | Y_LIM)
//	{
//		if(X_LIM)
//		{
//			X_STP_SET;
//			delay_us(50);
//			X_STP_RESET;
//		}
//
//		if(Y_LIM)
//		{
//			Y_STP_SET;
//			delay_us(50);
//			Y_STP_RESET;
//		}
//		delay_ms(2);
//	}


	set_current_start();

	CloseLoopUpdate();

//	PRINTF_UART0("locate_init\r\n");

	goto_null_point();

//	while(1)
//	{
//		if(fabs(g_TARM_RunRecord.pv_current.v - g_TARM_RunRecord.pv_distance.v) <= 0.02
//				&& fabs(g_TARM_RunRecord.pv_current.p - g_TARM_RunRecord.pv_distance.p)
//		<= 0.02)
//			break;
//	}
//	delay_ms(100);

	InitFlag = 1;
}
